/*
 * Team D regular autonomous code.
 * @author Derek Leung
 */
#include "state.c"

void automate();

int phase = 0;

void iso20();
void inter20();

task auto20() {
  if(routine==ISOLATE) {
    iso20();
  }
}

// dist to 11.5 in. => 18 in.
// dist to 20 in. => 38 in.
/*void iso20() {
  auto_arm(ARM_LVL1);
  auto_move(35.0);
  while(!drive_base_target_reached || !arm_target_reached) { } //wait for move to complete
  intake_direction = 1; //outtake continually
}*/

void iso20() {
  /*
  //drop arm and intake
  auto_intake(-1);
  auto_arm(ARM_L);

  //send the arm alert at 5 inches moved
  drive_base_alert = 5;

  //wait until arm reaches target
  while(!arm_target_reached) {}

  //target move to 18 inches
  auto_move(6);

  //wait for alert...
  while(drive_base_alert!=-1) {}

  //use encoders
  use_ultra = false;

  //raise arm to top and go to the scoring goal
  auto_arm(ARM_X);
  auto_move(500);

  //wait
  while(!arm_target_reached || !drive_base_target_reached) {}

  //outtake
  auto_intake(1);

  //wait for things to drop out (hard 1.5s)
  wait1Msec(1500);

  //go back, lower arm
  auto_move(-3000);
  auto_arm(ARM_L);
  */
}

void inter20() {
//  auto_move(48.0);
//  waitInMilliseconds(1500);
/*  switch(phase) {
    case 0:
      auto_move(48.0);
      auto_score(2, 3);
      automate();
      break;
    case 1:
      auto_move(-4.2);
      automate();
      break;
    case 2:
      auto_block();
      automate();
      break;
  }*/
}

void automate() {
  if(auto_state == READY) {
    phase++;
    phase = phase % 3;
  }
}
